#include <iostream>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <cstring>
#include <vector>
#include <cstdio>
#include <stdexcept>

class ImuSensor {
public:
    ImuSensor(const std::string& port) : uart_port(port), uart_fd(-1) {}

    bool init() {
        uart_fd = open(uart_port.c_str(), O_RDWR | O_NOCTTY);  // 注意：移除了 O_NDELAY
        if (uart_fd == -1) {
            perror("UART打开失败");
            throw std::runtime_error("无法打开串口设备");
        }

        struct termios options;
        if (tcgetattr(uart_fd, &options) != 0) {
            perror("获取串口属性失败");
            close(uart_fd);
            uart_fd = -1;
            return false;
        }

        // 配置波特率（根据IMU实际波特率修改，如 9600、115200、460800）
        cfsetispeed(&options, B115200);  // 输入波特率
        cfsetospeed(&options, B115200);  // 输出波特率

        // 数据位、校验、停止位（8N1，根据IMU协议调整）
        options.c_cflag &= ~CSIZE;
        options.c_cflag |= CS8;          // 8位数据位
        options.c_cflag &= ~PARENB;      // 无校验位
        options.c_cflag &= ~CSTOPB;      // 1位停止位

        // 原始模式（禁用终端处理）
        options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);  // 禁用规范模式
        options.c_oflag &= ~OPOST;                           // 禁用输出处理

        // 启用接收和本地模式
        options.c_cflag |= (CLOCAL | CREAD);  // 必须启用！

        // 超时和最小读取字节数（阻塞模式：等待至少1字节）
        options.c_cc[VMIN] = 1;   // 至少读取1字节
        options.c_cc[VTIME] = 0;  // 不等待超时（阻塞）

        // 应用配置
        if (tcsetattr(uart_fd, TCSANOW, &options) != 0) {
            perror("设置串口属性失败");
            close(uart_fd);
            uart_fd = -1;
            return false;
        }

        tcflush(uart_fd, TCIOFLUSH);  // 刷新缓冲区
        return true;
    }

    ssize_t read_raw_data(std::vector<uint8_t>& output) {
        if (uart_fd == -1) return -1;

        uint8_t buffer[256];
        ssize_t bytes_read = read(uart_fd, buffer, sizeof(buffer));
        if (bytes_read > 0) {
            output.insert(output.end(), buffer, buffer + bytes_read);
        }
        return bytes_read;
    }

    ~ImuSensor() {
        if (uart_fd != -1) {
            close(uart_fd);
            uart_fd = -1;
        }
    }

private:
    std::string uart_port;
    int uart_fd;
};

int main() {
    try {
        // 注意：替换为实际IMU连接的串口（如 /dev/ttyS0、/dev/ttyAMA0 等）
        ImuSensor sensor("/dev/ttyS0");  // 修改此处！

        if (!sensor.init()) {
            std::cerr << "串口初始化失败" << std::endl;
            return EXIT_FAILURE;
        }

        std::cout << "开始接收IMU数据（按 Ctrl+C 退出）..." << std::endl;

        while (true) {
            std::vector<uint8_t> imu_data;
            ssize_t bytes_read = sensor.read_raw_data(imu_data);

            if (bytes_read > 0) {
                std::cout << "接收到 " << bytes_read << " 字节数据: ";
                for (uint8_t byte : imu_data) {
                    printf("%02X ", byte);  // 十六进制输出
                }
                std::cout << std::endl;
            } else if (bytes_read == 0) {
                usleep(10000);  // 无数据时休眠
            } else {
                perror("读取数据失败");
                break;
            }
        }

    } catch (const std::exception& e) {
        std::cerr << "错误: " << e.what() << std::endl;
        return EXIT_FAILURE;
    }

    return EXIT_SUCCESS;
}